#include "Chassis_Task.h"

/************************************************************万能分隔符**************************************************************
 * 	@author:			//小瑞
 *	@performance:	    //头部PID+前馈总初始化函数
 *	@parameter:		    //
 *	@time:				//23-04-13 12:42
 *	@ReadMe:			//
 ************************************************************万能分隔符**************************************************************/
uint8_t RUI_F_MOTOR_PID_HEAD_INIT(MOTOR_Typdef *MOTOR)
{
    float PID_F_Pitch[3] = WHW_DF_PID_F_Chassis;
    float PID_P_Pitch[3] = WHW_DF_PID_P_Chassis;
    float PID_S_Pitch[3] = WHW_DF_PID_S_Chassis;

    //1
    Feedforward_Init(&MOTOR->DJI_3508_CHASSIS_1.PID_F, 3000, PID_F_Pitch,
                     0.5f, 2, 2);
    PID_Init(&MOTOR->DJI_3508_CHASSIS_1.PID_P, 6000.0f, 2000.0f,
             100.0f, PID_P_Pitch, 1000.0f, 1000.0f,
             0.7f, 0.7f, 2, Integral_Limit);
    PID_Init(&MOTOR->DJI_3508_CHASSIS_1.PID_S, 6000.0f, 2000.0f,
             100.0f, PID_S_Pitch, 1000.0f, 1000.0f,
             0.7f, 0.7f, 2, Integral_Limit);

    //2
    Feedforward_Init(&MOTOR->DJI_3508_CHASSIS_2.PID_F, 3000, PID_F_Pitch,
                     0.5f, 2, 2);
    PID_Init(&MOTOR->DJI_3508_CHASSIS_2.PID_P, 6000.0f, 2000.0f,
             100.0f, PID_P_Pitch, 1000.0f, 1000.0f,
             0.7f, 0.7f, 2, Integral_Limit);
    PID_Init(&MOTOR->DJI_3508_CHASSIS_2.PID_S, 6000.0f, 2000.0f,
             100.0f, PID_S_Pitch, 1000.0f, 1000.0f,
             0.7f, 0.7f, 2, Integral_Limit);

    //3
    Feedforward_Init(&MOTOR->DJI_3508_CHASSIS_3.PID_F, 3000, PID_F_Pitch,
                     0.5f, 2, 2);
    PID_Init(&MOTOR->DJI_3508_CHASSIS_3.PID_P, 6000.0f, 2000.0f,
             100.0f, PID_P_Pitch, 1000.0f, 1000.0f,
             0.7f, 0.7f, 2, Integral_Limit);
    PID_Init(&MOTOR->DJI_3508_CHASSIS_3.PID_S, 6000.0f, 2000.0f,
             100.0f, PID_S_Pitch, 1000.0f, 1000.0f,
             0.7f, 0.7f, 2, Integral_Limit);

    //4
    Feedforward_Init(&MOTOR->DJI_3508_CHASSIS_4.PID_F, 3000, PID_F_Pitch,
                     0.5f, 2, 2);
    PID_Init(&MOTOR->DJI_3508_CHASSIS_4.PID_P, 6000.0f, 2000.0f,
             100.0f, PID_P_Pitch, 1000.0f, 1000.0f,
             0.7f, 0.7f, 2, Integral_Limit);
    PID_Init(&MOTOR->DJI_3508_CHASSIS_4.PID_S, 6000.0f, 2000.0f,
             100.0f, PID_S_Pitch, 1000.0f, 1000.0f,
             0.7f, 0.7f, 2, Integral_Limit);

    return RUI_DF_READY;
}

uint8_t RUI_F_HEAD_AIM_INIT(RUI_ROOT_STATUS_Typedef *Root, MOTOR_Typdef *MOTOR)
{
    //检查离线
    if (Root->MOTOR_BOTTOM_CHASSIS_1     == RUI_DF_OFFLINE ||
        Root->MOTOR_BOTTOM_CHASSIS_2     == RUI_DF_OFFLINE ||
        Root->MOTOR_BOTTOM_CHASSIS_3     == RUI_DF_OFFLINE ||
        Root->MOTOR_BOTTOM_CHASSIS_4     == RUI_DF_OFFLINE)
        return RUI_DF_ERROR;

    //电机清空
    RUI_F_HEAD_MOTOR_CLEAR(&MOTOR->DJI_3508_CHASSIS_1, 1);
    RUI_F_HEAD_MOTOR_CLEAR(&MOTOR->DJI_3508_CHASSIS_2, 1);
    RUI_F_HEAD_MOTOR_CLEAR(&MOTOR->DJI_3508_CHASSIS_3, 1);
    RUI_F_HEAD_MOTOR_CLEAR(&MOTOR->DJI_3508_CHASSIS_4, 1);

    return RUI_DF_READY;
}

uint8_t chassis_task(CONTAL_Typedef *CONTAL,
                    RUI_ROOT_STATUS_Typedef *Root,
                    MOTOR_Typdef *MOTOR)
{
    static uint8_t PID_INIT=RUI_DF_ERROR;
    static uint8_t AIM_INIT = RUI_DF_ERROR;

    /*电机PID赋值*/
    if (PID_INIT != RUI_DF_READY)
    {
        PID_INIT = RUI_F_MOTOR_PID_HEAD_INIT(MOTOR);
        return RUI_DF_ERROR;
    }

    /*电机在线检测*/
    if (AIM_INIT != RUI_DF_READY)
    {
        AIM_INIT = RUI_F_HEAD_AIM_INIT(Root, MOTOR);
        return RUI_DF_ERROR;
    }

    /*底盘麦轮解算*/
    float wheel_rpm1 = (float)(-CONTAL->BOTTOM.VX + CONTAL->BOTTOM.VY + CONTAL->BOTTOM.VR);
    float wheel_rpm2 = (float)(+CONTAL->BOTTOM.VX + CONTAL->BOTTOM.VY + CONTAL->BOTTOM.VR);
    float wheel_rpm3 = (float)(+CONTAL->BOTTOM.VX - CONTAL->BOTTOM.VY + CONTAL->BOTTOM.VR);
    float wheel_rpm4 = (float)(-CONTAL->BOTTOM.VX - CONTAL->BOTTOM.VY + CONTAL->BOTTOM.VR);

    /*目标值赋值*/
    MOTOR->DJI_3508_CHASSIS_1.DATA.Aim = wheel_rpm1;
    MOTOR->DJI_3508_CHASSIS_2.DATA.Aim = wheel_rpm2;
    MOTOR->DJI_3508_CHASSIS_3.DATA.Aim = wheel_rpm3;
    MOTOR->DJI_3508_CHASSIS_4.DATA.Aim = wheel_rpm4;

    /*主机离线保护*/
    if(!Root->RM_Master)
    {
        MOTOR->DJI_3508_CHASSIS_1.PID_P.IntegralLimit = 0;
        MOTOR->DJI_3508_CHASSIS_1.PID_S.IntegralLimit = 0;
        MOTOR->DJI_3508_CHASSIS_1.DATA.Aim = (float)MOTOR->DJI_3508_CHASSIS_1.DATA.Angle_Infinite;

        MOTOR->DJI_3508_CHASSIS_2.PID_P.IntegralLimit = 0;
        MOTOR->DJI_3508_CHASSIS_2.PID_S.IntegralLimit = 0;
        MOTOR->DJI_3508_CHASSIS_2.DATA.Aim = (float)MOTOR->DJI_3508_CHASSIS_2.DATA.Angle_Infinite;

        MOTOR->DJI_3508_CHASSIS_3.PID_P.IntegralLimit = 0;
        MOTOR->DJI_3508_CHASSIS_3.PID_S.IntegralLimit = 0;
        MOTOR->DJI_3508_CHASSIS_3.DATA.Aim = (float)MOTOR->DJI_3508_CHASSIS_3.DATA.Angle_Infinite;

        MOTOR->DJI_3508_CHASSIS_4.PID_P.IntegralLimit = 0;
        MOTOR->DJI_3508_CHASSIS_4.PID_S.IntegralLimit = 0;
        MOTOR->DJI_3508_CHASSIS_4.DATA.Aim = (float)MOTOR->DJI_3508_CHASSIS_4.DATA.Angle_Infinite;

        PID_INIT=RUI_DF_ERROR;
    }

    /*堵转处理*/
    if(MOTOR->DJI_3508_CHASSIS_1.PID_P.ERRORHandler.ERRORType & Motor_Blocked)
        RUI_F_HEAD_MOTOR_CLEAR(&MOTOR->DJI_3508_CHASSIS_1, 0);
    if(MOTOR->DJI_3508_CHASSIS_2.PID_P.ERRORHandler.ERRORType & Motor_Blocked)
        RUI_F_HEAD_MOTOR_CLEAR(&MOTOR->DJI_3508_CHASSIS_2, 0);
    if(MOTOR->DJI_3508_CHASSIS_3.PID_P.ERRORHandler.ERRORType & Motor_Blocked)
        RUI_F_HEAD_MOTOR_CLEAR(&MOTOR->DJI_3508_CHASSIS_3, 0);
    if(MOTOR->DJI_3508_CHASSIS_4.PID_P.ERRORHandler.ERRORType & Motor_Blocked)
        RUI_F_HEAD_MOTOR_CLEAR(&MOTOR->DJI_3508_CHASSIS_4, 0);

    /*chassis1*/
    Feedforward_Calculate(&MOTOR->DJI_3508_CHASSIS_1.PID_F,
                          MOTOR->DJI_3508_CHASSIS_1.DATA.Aim);
    PID_Calculate(&MOTOR->DJI_3508_CHASSIS_1.PID_S,
                  MOTOR->DJI_3508_CHASSIS_1.DATA.Speed_now,
                  MOTOR->DJI_3508_CHASSIS_1.DATA.Aim);

    /*chassis2*/
    Feedforward_Calculate(&MOTOR->DJI_3508_CHASSIS_2.PID_F,
                          MOTOR->DJI_3508_CHASSIS_2.DATA.Aim);
    PID_Calculate(&MOTOR->DJI_3508_CHASSIS_2.PID_S,
                  MOTOR->DJI_3508_CHASSIS_2.DATA.Speed_now,
                  MOTOR->DJI_3508_CHASSIS_2.DATA.Aim);

    /*chassis3*/
    Feedforward_Calculate(&MOTOR->DJI_3508_CHASSIS_3.PID_F,
                          MOTOR->DJI_3508_CHASSIS_3.DATA.Aim);
    PID_Calculate(&MOTOR->DJI_3508_CHASSIS_3.PID_S,
                  MOTOR->DJI_3508_CHASSIS_3.DATA.Speed_now,
                  MOTOR->DJI_3508_CHASSIS_3.DATA.Aim);

    /*chassis4*/
    Feedforward_Calculate(&MOTOR->DJI_3508_CHASSIS_4.PID_F,
                          MOTOR->DJI_3508_CHASSIS_4.DATA.Aim);
    PID_Calculate(&MOTOR->DJI_3508_CHASSIS_4.PID_S,
                  MOTOR->DJI_3508_CHASSIS_4.DATA.Speed_now,
                  MOTOR->DJI_3508_CHASSIS_4.DATA.Aim);

    /*总输出计算*/
    float tmp1 = MOTOR->DJI_3508_CHASSIS_1.PID_F.Output +
                 MOTOR->DJI_3508_CHASSIS_1.PID_S.Output;

    float tmp2 = MOTOR->DJI_3508_CHASSIS_2.PID_F.Output +
                 MOTOR->DJI_3508_CHASSIS_2.PID_S.Output;

    float tmp3 = MOTOR->DJI_3508_CHASSIS_3.PID_F.Output +
                 MOTOR->DJI_3508_CHASSIS_3.PID_S.Output;

    float tmp4 = MOTOR->DJI_3508_CHASSIS_4.PID_F.Output +
                 MOTOR->DJI_3508_CHASSIS_4.PID_S.Output;

    DJI_Power_Ctrl(&hfdcan1,
                   0x200,
                   (int16_t)tmp1,
                   (int16_t)tmp2,
                   (int16_t)tmp3,
                   (int16_t)tmp4);

    return RUI_DF_READY;
}

